#include "png_pcd_path.hpp"

// 构造函数
PcdToDepthProcessor::PcdToDepthProcessor() 
    : original_cloud_(new PointCloudT), filtered_cloud_(new PointCloudT) {
    std::cout << "PcdToDepthProcessor initialized." << std::endl;
    std::cout << "Fixed depth image size: " << params_.image_width << "x" << params_.image_height << std::endl;
}

// 析构函数
PcdToDepthProcessor::~PcdToDepthProcessor() {
    std::cout << "PcdToDepthProcessor destroyed." << std::endl;
}

// 主处理函数
bool PcdToDepthProcessor::processPointCloud(const std::string& pcd_file) {
    std::cout << "\n=== Processing Point Cloud ===" << std::endl;
    
    // 1. 加载点云
    if (!loadPointCloud(pcd_file)) {
        return false;
    }
    
    // 2. 应用Z轴直通滤波
    applyZFilter();
    
    // 3. 应用体素滤波
    applyVoxelFilter();
    
    // 4. 应用半径滤波
    applyRadiusFilter();
    
    // 5. 计算点云边界
    calculatePointCloudBounds();
    
    // 6. 转换为深度图
    convertToDepthImage();
    
    // 7. 深度图预处理
    preprocessDepthImage();
    
    // 8. 提取边缘和路径
    extractEdgesAndPath();
    
    // 9. 保存结果
    saveResults();
    
    return true;
}

// 加载点云
bool PcdToDepthProcessor::loadPointCloud(const std::string& pcd_file) {
    if (pcl::io::loadPCDFile<PointT>(pcd_file, *original_cloud_) == -1) {
        std::cerr << "Error: Couldn't read PCD file: " << pcd_file << std::endl;
        return false;
    }
    
    std::cout << "Loaded " << original_cloud_->points.size() << " points." << std::endl;
    
    // 假设原始点云是有序的 3200x1400
    if (original_cloud_->width * original_cloud_->height != original_cloud_->points.size()) {
        std::cout << "Warning: Point cloud size (" << original_cloud_->points.size() 
                  << ") doesn't match expected organized size (" 
                  << params_.image_width << "x" << params_.image_height << "=" 
                  << (params_.image_width * params_.image_height) << ")" << std::endl;
    }
    
    // 初始化滤波索引数组
    filtered_indices_.resize(original_cloud_->points.size(), false);
    
    return true;
}

// 应用Z轴直通滤波
void PcdToDepthProcessor::applyZFilter() {
    std::cout << "Applying Z-axis filter [" << params_.z_min << ", " << params_.z_max << "]..." << std::endl;
    
    // 使用带索引的滤波
    pcl::PassThrough<PointT> pass_filter;
    pass_filter.setInputCloud(original_cloud_);
    pass_filter.setFilterFieldName("z");
    pass_filter.setFilterLimits(params_.z_min, params_.z_max);
    pass_filter.setKeepOrganized(false);
    
    // 获取滤波索引
    std::vector<int> indices;
    pass_filter.filter(indices);
    
    // 更新滤波索引数组
    for (int idx : indices) {
        if (idx >= 0 && idx < filtered_indices_.size()) {
            filtered_indices_[idx] = true;
        }
    }
    
    // 提取滤波后的点云
    pass_filter.filter(*filtered_cloud_);
    
    std::cout << "Filtered to " << filtered_cloud_->points.size() << " points." << std::endl;
}

// 应用体素滤波
void PcdToDepthProcessor::applyVoxelFilter() {
    std::cout << "Applying voxel grid filter (leaf size: " << params_.voxel_leaf_size << "m)..." << std::endl;
    
    if (filtered_cloud_->empty()) {
        std::cout << "Warning: No points to apply voxel filter on." << std::endl;
        return;
    }
    
    // 创建体素滤波器
    pcl::VoxelGrid<PointT> voxel_filter;
    voxel_filter.setInputCloud(filtered_cloud_);
    voxel_filter.setLeafSize(params_.voxel_leaf_size, params_.voxel_leaf_size, params_.voxel_leaf_size);
    
    // 应用体素滤波
    PointCloudT::Ptr voxel_filtered_cloud(new PointCloudT);
    voxel_filter.filter(*voxel_filtered_cloud);
    
    // 更新滤波后的点云
    filtered_cloud_ = voxel_filtered_cloud;
    
    std::cout << "Voxel filtered to " << filtered_cloud_->points.size() << " points." << std::endl;
}

// 应用半径滤波
void PcdToDepthProcessor::applyRadiusFilter() {
    std::cout << "Applying radius outlier removal filter (radius: " << params_.radius_search 
              << ", min neighbors: " << params_.min_neighbors << ")..." << std::endl;
    
    if (filtered_cloud_->empty()) {
        std::cout << "Warning: No points to apply radius filter on." << std::endl;
        return;
    }
    
    // 创建半径滤波器
    pcl::RadiusOutlierRemoval<PointT> radius_filter;
    radius_filter.setInputCloud(filtered_cloud_);
    radius_filter.setRadiusSearch(params_.radius_search);
    radius_filter.setMinNeighborsInRadius(params_.min_neighbors);
    radius_filter.setKeepOrganized(false);
    
    // 获取滤波索引
    std::vector<int> indices;
    radius_filter.filter(indices);
    
    // 创建临时点云来保存半径滤波结果
    PointCloudT::Ptr radius_filtered_cloud(new PointCloudT);
    radius_filter.filter(*radius_filtered_cloud);
    
    // 更新滤波后的点云
    filtered_cloud_ = radius_filtered_cloud;
    
    std::cout << "Radius filtered to " << filtered_cloud_->points.size() << " points." << std::endl;
    std::cout << "Filtered out " << (original_cloud_->points.size() - filtered_cloud_->points.size()) << " points." << std::endl;
}

// 计算点云边界
void PcdToDepthProcessor::calculatePointCloudBounds() {
    if (original_cloud_->empty()) {
        std::cerr << "Error: No original points!" << std::endl;
        return;
    }
    
    PointT min_pt, max_pt;
    pcl::getMinMax3D(*original_cloud_, min_pt, max_pt);
    
    params_.x_min = min_pt.x;
    params_.x_max = max_pt.x;
    params_.y_min = min_pt.y;
    params_.y_max = max_pt.y;
    
    std::cout << "Point cloud bounds: X[" << params_.x_min << ", " << params_.x_max 
              << "], Y[" << params_.y_min << ", " << params_.y_max << "]" << std::endl;
}

// 转换为深度图
void PcdToDepthProcessor::convertToDepthImage() {
    std::cout << "Converting to depth image..." << std::endl;
    
    // 创建固定尺寸的深度图 (3200x1400)
    depth_image_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32F);
    cv::Mat count_image = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32F);
    
    // 计算映射参数
    float x_range = params_.x_max - params_.x_min;
    float y_range = params_.y_max - params_.y_min;
    float x_scale = (params_.image_width - 1) / x_range;
    float y_scale = (params_.image_height - 1) / y_range;
    
    std::cout << "Mapping scale: X=" << x_scale << ", Y=" << y_scale << std::endl;
    
    // 遍历原始点云中的所有点
    for (size_t i = 0; i < original_cloud_->points.size(); ++i) {
        const auto& point = original_cloud_->points[i];
        
        // 跳过无效点
        if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)) {
            continue;
        }
        
        // 计算像素坐标
        int u = static_cast<int>((point.x - params_.x_min) * x_scale + 0.5f);
        int v = static_cast<int>((point.y - params_.y_min) * y_scale + 0.5f);
        
        // 确保坐标在图像范围内
        if (u >= 0 && u < params_.image_width && v >= 0 && v < params_.image_height) {
            if (filtered_indices_[i]) {
                // 直接使用Z值（保持负数特性），Z越大（越接近0）表示越近
                float depth_value = point.z;  // 保持原始Z值：Z=-80（近），Z=-115（远）
                depth_image_.at<float>(v, u) += depth_value;
                count_image.at<float>(v, u) += 1.0f;
            } else {
                // 该点被滤波掉，深度值设为0
                // 不需要操作，因为depth_image_已经初始化为0
            }
        }
    }
    
    // 对有多个点映射到同一像素的情况，计算平均深度值
    for (int v = 0; v < params_.image_height; ++v) {
        for (int u = 0; u < params_.image_width; ++u) {
            float count = count_image.at<float>(v, u);
            if (count > 0) {
                depth_image_.at<float>(v, u) /= count;
            }
        }
    }
    
    std::cout << "Depth image conversion completed." << std::endl;
}

// 深度图预处理
void PcdToDepthProcessor::preprocessDepthImage() {
    std::cout << "Preprocessing depth image..." << std::endl;
    
    if (depth_image_.empty()) {
        std::cerr << "Error: Depth image is empty!" << std::endl;
        return;
    }
    
    // 保存原始深度图用于比较
    cv::Mat original_depth = depth_image_.clone();
    
    // 1. 创建有效区域掩码（Z值为负数的区域）
    cv::Mat valid_mask = depth_image_ < 0;  // 有效Z值都是负数
    
    // 2. 先进行形态学操作过滤噪点
    std::cout << "  - Morphological operations to filter noise..." << std::endl;
    
    // 将深度图转换为8位用于形态学操作
    cv::Mat depth_8u_morph;
    double min_val, max_val;
    cv::minMaxLoc(depth_image_, &min_val, &max_val, nullptr, nullptr, valid_mask);
    
    if (max_val > min_val) {
        // Z值越大（越接近0）的地方越亮
        // min_val是最小的Z值（比如-115），max_val是最大的Z值（比如-80）
        cv::Mat normalized = (depth_image_ - static_cast<float>(min_val)) / static_cast<float>(max_val - min_val);
        normalized.convertTo(depth_8u_morph, CV_8U, 255.0);
        // 将无效区域设置为0
        depth_8u_morph.setTo(0, ~valid_mask);
    } else {
        depth_image_.convertTo(depth_8u_morph, CV_8U);
        depth_8u_morph.setTo(0, ~valid_mask);
    }
    
    // 形态学开运算：先腐蚀后膨胀，去除小的噪点
    cv::Mat morph_kernel = cv::getStructuringElement(cv::MORPH_RECT, 
                                                     cv::Size(params_.morphology_kernel_size, params_.morphology_kernel_size));
    cv::Mat opened;
    cv::morphologyEx(depth_8u_morph, opened, cv::MORPH_OPEN, morph_kernel);
    
    // 形态学闭运算：先膨胀后腐蚀，连接断开的区域
    cv::Mat morphed;
    cv::morphologyEx(opened, morphed, cv::MORPH_CLOSE, morph_kernel);
    
    // 转换回浮点深度图
    cv::Mat morphed_depth;
    morphed.convertTo(morphed_depth, CV_32F, (max_val - min_val) / 255.0, min_val);
    
    // 更新有效区域掩码（形态学操作后可能会改变有效区域）
    cv::Mat morphed_valid_mask = morphed > 0;
    
    // 3. 孔洞填补 - 只填充有效区域内的小孔洞，不填充纯黑区域
    std::cout << "  - Filling holes within valid regions only..." << std::endl;
    
    // 创建孔洞掩码：只考虑有效区域内的孔洞
    cv::Mat dilated_valid_mask;
    cv::Mat hole_kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
    cv::dilate(morphed_valid_mask, dilated_valid_mask, hole_kernel, cv::Point(-1, -1), 2);
    
    // 孔洞掩码 = 扩张后的有效区域 - 当前有效区域
    cv::Mat hole_mask = dilated_valid_mask & ~morphed_valid_mask;
    
    // 使用Telea算法进行图像修复，只修复孔洞区域
    cv::Mat inpainted_8u;
    cv::inpaint(morphed, hole_mask, inpainted_8u, 3, cv::INPAINT_TELEA);
    
    // 将修复后的图像转换回浮点深度图
    cv::Mat inpainted_depth;
    inpainted_8u.convertTo(inpainted_depth, CV_32F, (max_val - min_val) / 255.0, min_val);
    
    // 只在孔洞区域使用修复后的值，保持原有的有效区域和纯黑区域不变
    inpainted_depth.copyTo(morphed_depth, hole_mask);
    
    // 保存形态学处理后的结果
    cv::imwrite("depth_after_morphology.png", morphed);
    std::cout << "  - Saved: depth_after_morphology.png" << std::endl;
    
    // 4. 中值滤波去除孤立噪点
    std::cout << "  - Median filtering for noise removal..." << std::endl;
    
    cv::Mat median_filtered;
    cv::medianBlur(morphed_depth, median_filtered, params_.median_kernel_size);
    
    // 5. 双边滤波平滑同时保留边缘
    std::cout << "  - Bilateral filtering for edge-preserving smoothing..." << std::endl;
    
    // 将深度图转换为8位用于双边滤波
    cv::Mat depth_8u_bilateral;
    cv::minMaxLoc(median_filtered, &min_val, &max_val, nullptr, nullptr, median_filtered < 0);  // 有效值是负数
    
    if (max_val > min_val) {
        cv::Mat normalized = (median_filtered - static_cast<float>(min_val)) / static_cast<float>(max_val - min_val);
        normalized.convertTo(depth_8u_bilateral, CV_8U, 255.0);
        // 保持纯黑区域（无效区域）
        depth_8u_bilateral.setTo(0, median_filtered >= 0);  // Z>=0的区域是无效的
    } else {
        median_filtered.convertTo(depth_8u_bilateral, CV_8U);
        depth_8u_bilateral.setTo(0, median_filtered >= 0);
    }
    
    cv::Mat bilateral_filtered_8u;
    cv::bilateralFilter(depth_8u_bilateral, bilateral_filtered_8u, 
                        params_.bilateral_d, params_.bilateral_sigma_color, params_.bilateral_sigma_space);
    
    // 转换回浮点深度图
    bilateral_filtered_8u.convertTo(depth_image_, CV_32F, (max_val - min_val) / 255.0, min_val);
    
    // 确保无效区域保持为0
    depth_image_.setTo(0, bilateral_filtered_8u == 0);
    
    // 6. 保存预处理结果用于调试
    cv::Mat preprocessed_vis;
    cv::Mat final_valid_mask = depth_image_ < 0;  // 有效值是负数
    cv::minMaxLoc(depth_image_, &min_val, &max_val, nullptr, nullptr, final_valid_mask);
    
    if (max_val > min_val) {
        cv::Mat normalized = (depth_image_ - static_cast<float>(min_val)) / static_cast<float>(max_val - min_val);
        normalized.convertTo(preprocessed_vis, CV_8U, 255.0);
        // 保持无效区域为黑色
        preprocessed_vis.setTo(0, ~final_valid_mask);
    } else {
        depth_image_.convertTo(preprocessed_vis, CV_8U);
        preprocessed_vis.setTo(0, ~final_valid_mask);
    }
    cv::imwrite("depth_preprocessed.png", preprocessed_vis);
    
    // 保存对比图
    cv::Mat original_vis;
    cv::Mat original_valid_mask = original_depth < 0;  // 有效值是负数
    cv::minMaxLoc(original_depth, &min_val, &max_val, nullptr, nullptr, original_valid_mask);
    if (max_val > min_val) {
        cv::Mat normalized = (original_depth - static_cast<float>(min_val)) / static_cast<float>(max_val - min_val);
        normalized.convertTo(original_vis, CV_8U, 255.0);
        // 保持无效区域为黑色
        original_vis.setTo(0, ~original_valid_mask);
    } else {
        original_depth.convertTo(original_vis, CV_8U);
        original_vis.setTo(0, ~original_valid_mask);
    }
    cv::imwrite("depth_before_preprocessing.png", original_vis);
    
    // 统计预处理前后的有效像素数量
    int original_valid_pixels = cv::countNonZero(original_valid_mask);
    int final_valid_pixels = cv::countNonZero(final_valid_mask);
    
    std::cout << "Depth image preprocessing completed." << std::endl;
    std::cout << "  - Original valid pixels: " << original_valid_pixels << std::endl;
    std::cout << "  - Final valid pixels: " << final_valid_pixels << std::endl;
    std::cout << "  - Pixel change: " << (final_valid_pixels - original_valid_pixels) << std::endl;
    std::cout << "  - Saved: depth_before_preprocessing.png" << std::endl;
    std::cout << "  - Saved: depth_after_morphology.png" << std::endl;
    std::cout << "  - Saved: depth_preprocessed.png" << std::endl;
}

// 提取边缘和路径
void PcdToDepthProcessor::extractEdgesAndPath() {
    std::cout << "Extracting edges and gluing path..." << std::endl;
    
    if (depth_image_.empty()) {
        std::cerr << "Error: Depth image is empty!" << std::endl;
        return;
    }
    
    // 1. 将深度图转换为8位图像用于边缘检测
    cv::Mat depth_8u;
    double min_depth, max_depth;
    cv::Mat valid_mask = depth_image_ < 0;  // 有效值是负数
    cv::minMaxLoc(depth_image_, &min_depth, &max_depth, nullptr, nullptr, valid_mask);
    
    if (max_depth > min_depth) {  // max_depth是更大的Z值（更接近0，更近）
        // 直接归一化，让Z值越大（越接近0）的点越亮
        cv::Mat normalized = (depth_image_ - static_cast<float>(min_depth)) / static_cast<float>(max_depth - min_depth);
        normalized.setTo(0, ~valid_mask);
        normalized.convertTo(depth_8u, CV_8U, 255.0);
    } else {
        std::cout << "Warning: No valid depth values for edge detection!" << std::endl;
        return;
    }
    
    // 2. 应用高斯模糊减少噪声
    cv::Mat blurred;
    cv::GaussianBlur(depth_8u, blurred, cv::Size(params_.gaussian_kernel_size, params_.gaussian_kernel_size), 
                     params_.gaussian_sigma);
    
    // 3. Canny边缘检测
    cv::Mat edges;
    cv::Canny(blurred, edges, params_.canny_threshold1, params_.canny_threshold2);
    
    // 4. 形态学操作优化边缘
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
    cv::Mat edges_cleaned;
    cv::morphologyEx(edges, edges_cleaned, cv::MORPH_CLOSE, kernel);
    
    // 5. 寻找轮廓
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(edges_cleaned, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    
    // 6. 过滤和选择主要轮廓（鞋子内边缘）
    std::vector<std::vector<cv::Point>> filtered_contours;
    double min_contour_area = 500.0;  // 最小轮廓面积
    
    for (const auto& contour : contours) {
        double area = cv::contourArea(contour);
        if (area > min_contour_area) {
            filtered_contours.push_back(contour);
        }
    }
    
    
    // 7. 创建可视化图像
    cv::Mat path_visualization = cv::Mat::zeros(depth_image_.size(), CV_8UC3);
    
    // 绘制原始深度图作为背景
    cv::Mat depth_color_bg;
    cv::cvtColor(depth_8u, depth_color_bg, cv::COLOR_GRAY2BGR);  // 将灰度图转换为3通道BGR图
    depth_color_bg.copyTo(path_visualization);
    
    // 绘制检测到的路径
    //多个路经更换颜色
    for (size_t i = 0; i < filtered_contours.size(); ++i) {
        cv::Scalar color;
        if (i == 0) {
            color = cv::Scalar(0, 255, 0);  // 绿色 - 主要路径
        } else {
            color = cv::Scalar(0, 0, 255);  // 红色 - 次要路径
        }
        cv::drawContours(path_visualization, filtered_contours, static_cast<int>(i), color, 2);
    }
    
    // 8. 保存结果
    cv::imwrite("edges_detected.png", edges);
    cv::imwrite("edges_cleaned.png", edges_cleaned);
    cv::imwrite("gluing_path.png", path_visualization);
    

    // 9. 将路径坐标转换回3D空间并保存
    if (!filtered_contours.empty()) {
        savePathCoordinates(filtered_contours);
    }
}

// 保存路径坐标
void PcdToDepthProcessor::savePathCoordinates(const std::vector<std::vector<cv::Point>>& contours) {
    std::cout << "Saving path coordinates (target points per path: " << params_.path_point_count << ")..." << std::endl;
    
    // 保存文本格式的路径坐标
    std::ofstream path_file("gluing_path_3d.txt");
    if (!path_file.is_open()) {
        std::cerr << "Error: Cannot create path coordinate file!" << std::endl;
        return;
    }
    
    // 创建点云来保存所有路径点
    PointCloudT::Ptr all_paths_cloud(new PointCloudT);
    
    // 计算映射参数（与convertToDepthImage中的逆映射）
    float x_range = params_.x_max - params_.x_min;
    float y_range = params_.y_max - params_.y_min;
    float x_scale = (params_.image_width - 1) / x_range;
    float y_scale = (params_.image_height - 1) / y_range;
    
    path_file << "# Gluing Path 3D Coordinates\n";
    path_file << "# Format: path_id, x, y, z\n";
    path_file << "# Units: meters\n";
    path_file << "# Points per path: " << params_.path_point_count << "\n\n";
    
    for (size_t path_id = 0; path_id < contours.size(); ++path_id) {
        const auto& contour = contours[path_id];
        path_file << "# Path " << path_id << " (original points: " << contour.size() << ")\n";

        // 重新采样轮廓到指定点数
        std::vector<cv::Point> resampled_points = resampleContour(contour, params_.path_point_count);
        
        // 应用内偏移
        std::vector<cv::Point2f> offset_points = applyInwardOffset(resampled_points, params_.path_inward_offset);
        
        // 创建单个路径的点云
        PointCloudT::Ptr single_path_cloud(new PointCloudT);
        single_path_cloud->width = offset_points.size();
        single_path_cloud->height = 1;
        single_path_cloud->is_dense = false;
        single_path_cloud->resize(offset_points.size());
        
        int valid_points = 0;  // 统计有效点数
        
        for (size_t i = 0; i < offset_points.size(); ++i) {
            const auto& point = offset_points[i];
            
            // 将像素坐标转换回3D坐标
            float x_3d = params_.x_min + (point.x / x_scale);
            float y_3d = params_.y_min + (point.y / y_scale);
            
            // 获取对应的深度值（使用改进的深度获取函数）
            float depth_value = getDepthAtPixel(static_cast<int>(point.x + 0.5f), 
                                              static_cast<int>(point.y + 0.5f));
            
            // 将深度值转换回Z坐标（现在depth_value就是原始的Z值）
            float z_3d = depth_value;  // 直接使用深度值，保持负数特性
            
            // 只保存有效的3D点（有有效深度的点，忽略深度为0的点）
            if (depth_value < 0) {  // 有效深度值（负数且不为0）
                valid_points++;
                
                // 保存到文本文件
                path_file << path_id << ", " << std::fixed << std::setprecision(6) 
                         << x_3d << ", " << y_3d << ", " << z_3d << "\n";
                
                // 添加到单个路径点云
                PointT pcl_point;
                pcl_point.x = x_3d;
                pcl_point.y = y_3d;
                pcl_point.z = z_3d;
                single_path_cloud->points[i] = pcl_point;
                
                // 添加到总点云
                all_paths_cloud->points.push_back(pcl_point);
            } else {
                // 无效深度值，创建一个占位点（可以选择跳过或使用默认值）
                PointT pcl_point;
                pcl_point.x = x_3d;
                pcl_point.y = y_3d;
                pcl_point.z = 0.0f;  // 使用0作为无效深度的标记
                single_path_cloud->points[i] = pcl_point;
                
                // 在文本文件中记录但标注为无效
                path_file << path_id << ", " << std::fixed << std::setprecision(6) 
                         << x_3d << ", " << y_3d << ", 0.000000 # invalid_depth\n";
            }
        }
        
        // 保存单个路径的PCD文件
        std::string single_path_filename = "gluing_path_" + std::to_string(path_id) + ".pcd";
        pcl::io::savePCDFileBinary(single_path_filename, *single_path_cloud);
        std::cout << "  - Saved path " << path_id << " (" << offset_points.size() 
                  << " total points, " << valid_points << " valid) to: " << single_path_filename << std::endl;
        
        path_file << "\n";
    }
    
    path_file.close();
    
    // 创建路径点可视化图像
    std::cout << "Creating path points visualization..." << std::endl;
    
    // 1. 创建基于深度图的背景图像
    cv::Mat visualization_bg;
    cv::Mat valid_mask = depth_image_ < 0;  // 有效值是负数
    double min_depth, max_depth;
    cv::minMaxLoc(depth_image_, &min_depth, &max_depth, nullptr, nullptr, valid_mask);
    
    if (max_depth > min_depth) {
        // 创建归一化的深度图作为背景
        cv::Mat normalized = (depth_image_ - static_cast<float>(min_depth)) / static_cast<float>(max_depth - min_depth);
        normalized.setTo(0, ~valid_mask);
        normalized.convertTo(visualization_bg, CV_8U, 255.0);
        
        // 转换为3通道BGR图像
        cv::cvtColor(visualization_bg, visualization_bg, cv::COLOR_GRAY2BGR);
    } else {
        // 如果没有有效深度值，创建黑色背景
        visualization_bg = cv::Mat::zeros(params_.image_height, params_.image_width, CV_8UC3);
    }
    
    // 2. 在背景图上绘制所有路径的采样点
    for (size_t path_id = 0; path_id < contours.size(); ++path_id) {
        const auto& contour = contours[path_id];
        
        // 重新采样轮廓到指定点数
        std::vector<cv::Point> resampled_points = resampleContour(contour, params_.path_point_count);
        
        // 应用内偏移
        std::vector<cv::Point2f> offset_points = applyInwardOffset(resampled_points, params_.path_inward_offset);
        
        // 绘制路径点（红色圆圈）
        for (size_t i = 0; i < offset_points.size(); ++i) {
            cv::Point pixel_point(static_cast<int>(offset_points[i].x + 0.5f), 
                                static_cast<int>(offset_points[i].y + 0.5f));
            
            // 确保点在图像范围内
            if (pixel_point.x >= 0 && pixel_point.x < params_.image_width && 
                pixel_point.y >= 0 && pixel_point.y < params_.image_height) {
                
                // 检查该点是否有有效深度值
                float depth_value = getDepthAtPixel(pixel_point.x, pixel_point.y);
                
                if (depth_value < 0) {  // 有效深度值
                    // 绘制红色实心圆点
                    cv::circle(visualization_bg, pixel_point, 3, cv::Scalar(0, 0, 255), -1);  // BGR格式：红色
                    
                    // 为第一个和最后一个点添加特殊标记
                    if (i == 0) {
                        // 起始点：绿色外圈
                        cv::circle(visualization_bg, pixel_point, 5, cv::Scalar(0, 255, 0), 2);
                        cv::putText(visualization_bg, "S", 
                                  cv::Point(pixel_point.x - 8, pixel_point.y - 8), 
                                  cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 255, 0), 1);
                    } else if (i == offset_points.size() - 2) {
                        // 终点：蓝色外圈
                        cv::circle(visualization_bg, pixel_point, 5, cv::Scalar(255, 0, 0), 2);
                        cv::putText(visualization_bg, "E", 
                                  cv::Point(pixel_point.x - 8, pixel_point.y - 8), 
                                  cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 0, 0), 1);
                    }
                } else {
                    // 无效深度值的点用灰色标记
                    cv::circle(visualization_bg, pixel_point, 2, cv::Scalar(128, 128, 128), -1);
                }
            }
        }
        
        // 绘制路径编号
        if (!offset_points.empty()) {
            cv::Point label_pos(static_cast<int>(offset_points[0].x + 10), 
                              static_cast<int>(offset_points[0].y - 10));
            
            // 确保标签位置在图像范围内
            label_pos.x = std::max(0, std::min(params_.image_width - 50, label_pos.x));
            label_pos.y = std::max(20, std::min(params_.image_height - 5, label_pos.y));
            
            std::string path_label = "Path " + std::to_string(path_id);
            cv::putText(visualization_bg, path_label, label_pos, 
                       cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 255, 0), 2);  // 黄色文字
        }
    }
    // 4. 保存可视化图像
    std::string visualization_filename = "path_points_visualization.png";
    cv::imwrite(visualization_filename, visualization_bg);
    std::cout << "Saved path points visualization to: " << visualization_filename << std::endl;
    
    // 保存所有路径的合并点云
    if (!all_paths_cloud->points.empty()) {
        all_paths_cloud->width = all_paths_cloud->points.size();
        all_paths_cloud->height = 1;
        all_paths_cloud->is_dense = false;
        
        pcl::io::savePCDFileBinary("gluing_paths_all.pcd", *all_paths_cloud);
        std::cout << "Saved all paths combined (" << all_paths_cloud->points.size() 
                  << " points) to: gluing_paths_all.pcd" << std::endl;
    }

}




///////////////////////////////////////////////////////////////////////////////////
// 轮廓重新采样函数 - 将轮廓均匀重采样到指定点数
std::vector<cv::Point> PcdToDepthProcessor::resampleContour(const std::vector<cv::Point>& contour, int target_points) {
    if (contour.size() <= 1 || target_points <= 1) {
        return contour;
    }
    
    // 计算轮廓的累积弧长
    std::vector<double> cumulative_lengths;
    cumulative_lengths.push_back(0.0);
    
    double total_length = 0.0;
    for (size_t i = 1; i < contour.size(); ++i) {
        double dx = contour[i].x - contour[i-1].x;
        double dy = contour[i].y - contour[i-1].y;
        double segment_length = std::sqrt(dx * dx + dy * dy);
        total_length += segment_length;
        cumulative_lengths.push_back(total_length);
    }
    
    if (total_length < 1e-6) {
        // 轮廓长度太小，直接返回原轮廓
        return contour;
    }
    
    // 均匀采样
    std::vector<cv::Point> resampled_points;
    resampled_points.reserve(target_points);
    
    for (int i = 0; i < target_points; ++i) {
        double target_length = (static_cast<double>(i) / (target_points - 1)) * total_length;
        
        // 找到目标长度对应的线段
        size_t segment_idx = 0;
        for (size_t j = 1; j < cumulative_lengths.size(); ++j) {
            if (cumulative_lengths[j] >= target_length) {
                segment_idx = j - 1;
                break;
            }
        }
        
        if (segment_idx >= contour.size() - 1) {
            // 最后一个点
            resampled_points.push_back(contour.back());
        } else {
            // 在线段上插值
            double segment_start_length = cumulative_lengths[segment_idx];
            double segment_end_length = cumulative_lengths[segment_idx + 1];
            
            if (std::abs(segment_end_length - segment_start_length) < 1e-6) {
                // 线段长度为0
                resampled_points.push_back(contour[segment_idx]);
            } else {
                double t = (target_length - segment_start_length) / (segment_end_length - segment_start_length);
                t = std::max(0.0, std::min(1.0, t)); // 限制在[0,1]范围内
                
                cv::Point interpolated_point;
                interpolated_point.x = static_cast<int>(contour[segment_idx].x + t * (contour[segment_idx + 1].x - contour[segment_idx].x) + 0.5);
                interpolated_point.y = static_cast<int>(contour[segment_idx].y + t * (contour[segment_idx + 1].y - contour[segment_idx].y) + 0.5);
                resampled_points.push_back(interpolated_point);
            }
        }
    }
    
    return resampled_points;
}

// 获取指定像素位置的深度值（在5像素半径内搜索最大深度值）
float PcdToDepthProcessor::getDepthAtPixel(int x, int y) const {
    if (x < 0 || x >= params_.image_width || y < 0 || y >= params_.image_height) {
        return 0.0f;
    }
    
    const int search_radius = 20;  // 搜索半径：5个像素
    float max_depth = 0.0f;       // 最大深度值（在负数系统中，越大越接近相机）
    bool found_valid_depth = false;
    
    // 在指定半径内搜索所有像素
    for (int dy = -search_radius; dy <= search_radius; ++dy) {
        for (int dx = -search_radius; dx <= search_radius; ++dx) {
            // 检查是否在圆形区域内（使用欧几里得距离）
            float distance = std::sqrt(dx * dx + dy * dy);
            if (distance > search_radius) {
                continue;  // 超出圆形搜索范围
            }
            
            int search_x = x + dx;
            int search_y = y + dy;
            
            // 确保搜索坐标在图像范围内
            if (search_x >= 0 && search_x < params_.image_width && 
                search_y >= 0 && search_y < params_.image_height) {
                
                float depth_value = depth_image_.at<float>(search_y, search_x);
                
                // 只考虑有效的深度值（负数且不为0）
                if (depth_value < 0) {
                    if (!found_valid_depth || depth_value > max_depth) {
                        max_depth = depth_value;  // 更新最大深度值（更接近0的负数）
                        found_valid_depth = true;
                    }
                }
            }
        }
    }
    
    // 如果在搜索半径内找到了有效深度值，返回最大值
    if (found_valid_depth) {
        return max_depth;
    }
    
    // 最终返回0表示无法找到有效深度值
    return 0.0f;
}


// 将轮廓向内偏移指定像素距离
std::vector<cv::Point2f> PcdToDepthProcessor::applyInwardOffset(const std::vector<cv::Point>& contour, float offset_pixels) {
    std::vector<cv::Point2f> offset_points;
    
    if (contour.size() < 3 || offset_pixels <= 0) {
        // 如果轮廓点太少或偏移为0，直接转换为float类型返回
        for (const auto& pt : contour) {
            offset_points.emplace_back(static_cast<float>(pt.x), static_cast<float>(pt.y));
        }
        return offset_points;
    }
    
    offset_points.reserve(contour.size());
    
    for (size_t i = 0; i < contour.size(); ++i) {
        // 获取当前点和相邻点
        size_t prev_idx = (i == 0) ? contour.size() - 1 : i - 1;
        size_t next_idx = (i == contour.size() - 1) ? 0 : i + 1;
        
        cv::Point2f curr(static_cast<float>(contour[i].x), static_cast<float>(contour[i].y));
        cv::Point2f prev(static_cast<float>(contour[prev_idx].x), static_cast<float>(contour[prev_idx].y));
        cv::Point2f next(static_cast<float>(contour[next_idx].x), static_cast<float>(contour[next_idx].y));
        
        // 计算两个边的方向向量
        cv::Point2f dir1 = curr - prev;
        cv::Point2f dir2 = next - curr;
        
        // 计算方向向量的长度并归一化
        float len1 = std::sqrt(dir1.x * dir1.x + dir1.y * dir1.y);
        float len2 = std::sqrt(dir2.x * dir2.x + dir2.y * dir2.y);
        
        if (len1 < 1e-6 || len2 < 1e-6) {
            // 如果边长度太小，直接使用原点
            offset_points.push_back(curr);
            continue;
        }
        
        dir1 /= len1;
        dir2 /= len2;
        
        // 计算内法向量（垂直向量）
        cv::Point2f normal1(-dir1.y, dir1.x);  // dir1的左法向量
        cv::Point2f normal2(-dir2.y, dir2.x);  // dir2的左法向量
        
        // 计算平均法向量
        cv::Point2f avg_normal = (normal1 + normal2) * 0.5f;
        
        // 归一化平均法向量
        float normal_len = std::sqrt(avg_normal.x * avg_normal.x + avg_normal.y * avg_normal.y);
        if (normal_len < 1e-6) {
            // 如果平均法向量长度为0（可能是180度转角），使用dir1的法向量
            avg_normal = normal1;
            normal_len = 1.0f;
        } else {
            avg_normal /= normal_len;
        }
        
        // 检查法向量是否指向轮廓内部
        // 使用叉积判断轮廓的方向（顺时针还是逆时针）
        float cross_product = dir1.x * dir2.y - dir1.y * dir2.x;
        
        // 如果轮廓是顺时针方向，需要反转法向量方向
        if (cross_product > 0) {
            avg_normal = -avg_normal;
        }
        
        // 应用偏移
        cv::Point2f offset_point = curr + avg_normal * offset_pixels;
        offset_points.push_back(offset_point);
    }
    
    return offset_points;
}

// 计算深度图的质心位置（基于有效像素的加权质心）
cv::Point2f PcdToDepthProcessor::calculateDepthImageCentroid() const {
    double sum_x = 0.0, sum_y = 0.0, sum_weight = 0.0;
    
    for (int y = 0; y < params_.image_height; ++y) {
        for (int x = 0; x < params_.image_width; ++x) {
            float depth_val = depth_image_.at<float>(y, x);
            
            // 只考虑有效的深度值（负数且不为0）
            if (depth_val < 0) {
                // 使用深度值的绝对值作为权重，越接近的点权重越大
                double weight = -depth_val;  // 转为正数，Z=-80权重80，Z=-100权重100
                weight = 1.0 / weight;       // 取倒数，越近的点权重越大
                
                sum_x += x * weight;
                sum_y += y * weight;
                sum_weight += weight;
            }
        }
    }
    
    if (sum_weight > 0) {
        return cv::Point2f(static_cast<float>(sum_x / sum_weight), 
                          static_cast<float>(sum_y / sum_weight));
    } else {
        // 如果没有有效深度值，返回图像中心
        return cv::Point2f(params_.image_width * 0.5f, params_.image_height * 0.5f);
    }
}

// 保存结果
void PcdToDepthProcessor::saveResults() {
    std::cout << "Saving results..." << std::endl;
    
    // 保存滤波后的点云（检查是否为空）
    if (filtered_cloud_->empty()) {
        std::cout << "Warning: Filtered point cloud is empty! Skipping PCD file save." << std::endl;
    } else {
        pcl::io::savePCDFileBinary("filtered.pcd", *filtered_cloud_);
        std::cout << "Saved: filtered.pcd" << std::endl;
    }
    
    // 分析深度图统计信息
    double min_depth, max_depth;
    cv::Mat valid_mask = depth_image_ < 0;  // 有效值是负数
    cv::minMaxLoc(depth_image_, &min_depth, &max_depth, nullptr, nullptr, valid_mask);
    
    int total_pixels = params_.image_width * params_.image_height;
    int valid_pixels = cv::countNonZero(valid_mask);
    int zero_pixels = total_pixels - valid_pixels;
    
    
    if (max_depth > min_depth) {  // 现在max_depth是更大的Z值（更接近0）
        // 保存归一化到0-255的深度图
        cv::Mat depth_normalized_8u;
        if (max_depth - min_depth < 1e-6) {
            // 深度值几乎相同，直接将有效像素设为 255
            depth_normalized_8u = cv::Mat::zeros(depth_image_.size(), CV_8U);
            depth_normalized_8u.setTo(255, valid_mask);
        } else {
            // 直接归一化，让Z值越大（越接近0，越近）的点越亮
            cv::Mat normalized = (depth_image_ - static_cast<float>(min_depth)) / static_cast<float>(max_depth - min_depth);
            normalized.setTo(0, ~valid_mask); // 将无效像素置为0
            normalized.convertTo(depth_normalized_8u, CV_8U, 255.0);
        }
        cv::imwrite("depth_normalized_0_255.png", depth_normalized_8u);
        std::cout << "Saved: depth_normalized_0_255.png (8-bit, 0-255 range, Z越大越亮)" << std::endl;

        // 保存彩色深度图可视化版本
        cv::Mat depth_color;
        cv::applyColorMap(depth_normalized_8u, depth_color, cv::COLORMAP_JET);
        cv::imwrite("depth_color.png", depth_color);
        std::cout << "Saved: depth_color.png (colorized visualization)" << std::endl;
    } else {
        std::cout << "Warning: No valid depth values found!" << std::endl;
    }
}

bool fileExists(const std::string& filename) {
    std::ifstream file(filename);
    return file.good();
}

int main(int argc, char* argv[]) {
    if (argc != 2) {
        std::cout << "Usage: " << argv[0] << " <pcd_file>" << std::endl;
        return -1;
    }
    
    std::string pcd_file = argv[1];
    
    if (!fileExists(pcd_file)) {
        std::cerr << "Error: File not found: " << pcd_file << std::endl;
        return -1;
    }
    
    PcdToDepthProcessor processor;
    
    if (!processor.processPointCloud(pcd_file)) {
        std::cerr << "Processing failed!" << std::endl;
        return -1;
    }
    
    std::cout << "Processing completed!" << std::endl;
    return 0;
}
